(* # ===================================================================
   # Matrix Project
   # Copyright FEM-NUAA.CN 2020
   # =================================================================== *)


Require Import Reals.
Open Scope R_scope.
Require Export Matrix.Mat.RMatrix.
Require Export Matrix.Mat.RMtacs.

(** Sb -> Sg *)

(* 偏航角ᴪ   *)
Parameter psi  : R.

(* 俯仰角θ   *)
Parameter theta: R.

(* 滚动角Φ  *)
Parameter phi : R.


(* 由 机体坐标轴系Sb 转动 滚动角Φ 到 过度坐标轴系S’’ *)
Definition coordinate_transform_SbS'' :  Mat R 3 3 := mkMat_3_3
  1         0          0
  0     (cos phi) (-sin phi)
  0     (sin phi) ( cos phi).

(* 由 过度坐标轴系S''转动 俯仰角θ 到 过度坐标轴系S’ *)
Definition coordinate_transform_S''S' : Mat R 3 3 := mkMat_3_3
  (cos theta)    0  (sin theta)
       0         1      0     
  (-sin theta)   0  (cos theta).

(* 由 过度坐标轴系S’ 转动 偏航角ᴪ  到 地面坐标轴系Sg *)
Definition coordinate_transform_S'Sg : Mat R 3 3 := mkMat_3_3
  (cos psi)   (-sin psi)   0
  (sin psi)   (cos psi)    0
     0            0        1.


Definition coordinate_transform_SbSg : Mat R 3 3 := mkMat_3_3
  ((cos theta)*(cos psi)) ((sin theta)*(cos psi)*(sin phi)-(sin psi)*(cos phi)) ((sin theta)*(cos psi)*(cos phi)+(sin psi)*(sin phi))
  ((cos theta)*(sin psi)) ((sin theta)*(sin psi)*(sin phi)+(cos psi)*(cos phi)) ((sin theta)*(sin psi)*(cos phi)-(cos psi)*(sin phi))
       ( -sin theta)                  ((cos theta)*(sin phi))                              ((cos theta)*(cos phi)).

Definition transition_S'Sg_mul_S''S' : Mat R 3 3 := mkMat_3_3
  ((cos psi)*(cos theta))  (-sin psi)   ((cos psi)*(sin theta))
  ((sin psi)*(cos theta))  ( cos psi)   ((sin psi)*(sin theta))
        (-sin theta)           0          (cos theta).

Lemma transition_S'Sg_mul_S''S'_eq:
transition_S'Sg_mul_S''S' === RMmul coordinate_transform_S'Sg coordinate_transform_S''S'.
Proof.
  unfold transition_S'Sg_mul_S''S'.
  RMat_mul_simpl. unfold mkMat_3_3'.
  f_equal3.
Qed.

(* S''Sg * S''S' * SbS'' = transition_S'Sg_mul_S''S' * SbS'' *)
Definition transition_S'Sg_mul_S''S'_mul_SbS'' :=
  RMmul transition_S'Sg_mul_S''S' coordinate_transform_SbS''.


(* verify  S''Sb * S'S'' * SgS' = SbSg *)
Lemma coordinate_transform_SbSg_eq :
  coordinate_transform_SbSg === transition_S'Sg_mul_S''S'_mul_SbS''.
Proof.
  unfold coordinate_transform_SbSg.
  unfold transition_S'Sg_mul_S''S'_mul_SbS''.
  RMat_mul_simpl. unfold mkMat_3_3'. f_equal2. ring.
  f_equal. ring. f_equal. ring. f_equal. ring. f_equal. ring.
  f_equal. ring.
Qed.

